#include "XL430W250.h"
#include "oslib.h"
#include <stdbool.h>
//#include "usart3.h"
#include "dynamixel2.h"
#include "usart.h"
#define DxlBaudrate 57600
#define ID 1
#define TorqueEnable 64
#define GoalPosition 596
#define GoalTorque 116
#define PresentPosition 611
#define PresentVelocity 615
#define PresentCurrent 621

uint8_t send_buff[100];
unsigned int i = 0;

void Servo_Torque(uint8_t id, bool TrqState)
{
	// if (TrqState != 1 || TrqState != 0)
	// {
	// 	uprintf("state should be 1 or 0\n\r");
	// 	return;
	// }
	dxl2_write_byte(id, 0x40, TrqState);
	dxl2_tx_packet();
	osDelay(30);
	dxl2_write_byte(id, 0x41, TrqState);
	dxl2_tx_packet();
	osDelay(30);
	uprintf("%d Dynamixel to %d\n\r", id, TrqState);
}

void Servo_Goal_Position(uint8_t id, uint32_t pos)
{
	dxl2_write_dword(id, 0x74, pos);
	dxl2_tx_packet();
	osDelay(30);
	uprintf("id %d pos %d\n\r", id, pos);
}

void TrqEn(bool TrqState) // 鑸垫満涓婂姏鐭╁拰led鐏�
{

	dxl2_write_byte(0, 0x41, TrqState);
	dxl2_tx_packet();
	osDelay(500);
	dxl2_write_byte(0, 0x40, TrqState);
	dxl2_tx_packet();
	osDelay(500);
	dxl2_write_byte(1, 0x41, TrqState);
	dxl2_tx_packet();
	osDelay(500);
	dxl2_write_byte(1, 0x40, TrqState);
	dxl2_tx_packet();
	osDelay(500);
	dxl2_write_byte(2, 0x41, TrqState);
	dxl2_tx_packet();
	osDelay(500);
	dxl2_write_byte(2, 0x40, TrqState);
	dxl2_tx_packet();

	// dxl2_write_byte(1, 0x41, 0);
	// dxl2_tx_packet();
	// osDelay(500);
	// dxl2_write_byte(1, 0x41, 1);
	// dxl2_tx_packet();
	// osDelay(500);
	// dxl2_write_byte(1, 0x41, 0);
	// dxl2_tx_packet();
	// osDelay(500);
	// dxl2_write_byte(1, 0x41, 1);
	// dxl2_tx_packet();
	// osDelay(500);
	// dxl2_write_byte(1, 0x41, 0);
	// dxl2_tx_packet();
	// osDelay(500);
	// dxl2_write_byte(1, 0x41, 1);
	// dxl2_tx_packet();
	// osDelay(500);
	// dxl2_write_byte(1, 0x41, 0);
	// dxl2_tx_packet();
	// osDelay(500);
	// dxl2_write_byte(1, 0x41, 1);
	// dxl2_tx_packet();
	// osDelay(500);
}

void Led(int times) //鑸垫満LED鎺у埗
{
	dxl2_write_byte(0, 0x41, 1);
	dxl2_tx_packet();
	osDelay(500);
	dxl2_write_byte(0, 0x41, 0);
	dxl2_tx_packet();
	osDelay(500);
	dxl2_write_byte(1, 0x41, 1);
	dxl2_tx_packet();
	osDelay(500);
	dxl2_write_byte(1, 0x41, 0);
	dxl2_tx_packet();
	osDelay(500);
	dxl2_write_byte(2, 0x41, 1);
	dxl2_tx_packet();
	osDelay(500);
	dxl2_write_byte(2, 0x41, 0);
	dxl2_tx_packet();
	osDelay(500);

	// for (int i = 0; i < 3; i++)
	// {
	// 	dxl2_write_byte(times, 0x41, 0);
	// 	dxl2_tx_packet();
	// 	osDelay(500);
	// 	dxl2_write_byte(times, 0x41, 1);
	// 	dxl2_tx_packet();
	// 	osDelay(500);
	// }
}
void ReadPos(void)
{
	TrqEn(1);
	dxl2_write_byte(1, 0x41, 1);
	dxl2_tx_packet();
	// dxl2_read_dword(3, 0x84);
	// dxl2_tx_packet();
	osDelay(30);
}
void Run(void)
{

	TrqEn(1); //鑸垫満涓婂姏鐭�
	for (i = 0; i < 10; i++)
	{
		osDelay(30);
		dxl2_write_dword(0, 0x74, i * 50);
		dxl2_tx_packet();
		osDelay(30);
		dxl2_write_dword(1, 0x74, i * 50);
		dxl2_tx_packet();
		osDelay(30);
		dxl2_write_dword(2, 0x74, i * 50);
		dxl2_tx_packet();
	}
	for (i = 10; i > 0; i--)
	{
		osDelay(30);
		dxl2_write_dword(0, 0x74, i * 50);
		dxl2_tx_packet();
		osDelay(30);
		dxl2_write_dword(1, 0x74, i * 50);
		dxl2_tx_packet();
		osDelay(30);
		dxl2_write_dword(2, 0x74, i * 50);
		dxl2_tx_packet();
	}
}
void Stop(void)
{
}
void Right(void)
{
}
void Left(void)
{
}
void Back(void)
{
}

// void Usart2Receive_IDLE(void)
// {
// 	//__HAL_UART_CLEAR_IDLEFLAG(huart);
// 	HAL_UART_Transmit(&huart2, aRxBuffer2, 1, 100); // 鎺ユ敹鍒版暟鎹┈涓婁娇鐢ㄤ覆鍙�1鍙戦€佸嚭鍘�
// 	HAL_UART_Receive_IT(&huart2, aRxBuffer2, 1);	// 閲嶆柊浣胯兘涓插彛2鎺ユ敹涓柇
// }


unsigned char usart2_rxbuff[USART2_BUFFER_SIZE];
uint8_t usart2_rx_flag = 0;
uint8_t usart2_rx_buffer[128];

void usart2_init()
{
	/* USART2鎺ユ敹 */
	if (HAL_UART_Receive_DMA(&huart2, (uint8_t *)&usart2_rxbuff, USART2_BUFFER_SIZE) != HAL_OK)
	{
		Error_Handler();
	}
	/* 寮€鍚┖闂叉帴鏀朵腑鏂� */
	__HAL_UART_ENABLE_IT(&huart2, UART_IT_IDLE);
}

void Usart2Receive_IDLE(UART_HandleTypeDef *huart)
{
	//uint16_t i = 0;

	if ((__HAL_UART_GET_FLAG(huart, UART_FLAG_IDLE) != RESET))
	{
		if (huart->Instance == USART2)
		{
			__HAL_UART_CLEAR_IDLEFLAG(huart);
			i = huart->Instance->SR;
			i = huart->Instance->DR;
			i = hdma_usart2_rx.Instance->NDTR;
			/* ???茅?????????????篓HAL_UART_DMAStop(huart)?????????????????掳?????????TX???DMA??????茅??????掳???????DMA???茅??????????掳??? */
			/* 2020.10.24 ?????????HAL_UART_AbortReceive(huart)?????????DMA???茅?????茅???????????????陇???? */
			HAL_UART_AbortReceive(huart);

			/* ??陇?陇??陇??????掳?????????????????藝????????????????????? */
			// if (usart2_rx_flag == 0)
			// {
			// 	memcpy(vp_txbuff, usart2_rxbuff, (USART2_BUFFER_SIZE - i));
			// 	usart2_rx_flag = 1;
			// }

			/* ???????????????茅????掳?????? */
			// memset(usart2_rxbuff, 0x00, USART2_BUFFER_SIZE);
			HAL_UART_Receive_DMA(huart, (uint8_t *)&usart2_rxbuff, USART2_BUFFER_SIZE);
		}
	}
}

void usart2_handle_exe()
{
	
}